/***************************************************************************
创建者：彭秋明
 开始时间：         : 2017.02.2７
 copyright            : (C) 同川科技
 修改说明：（每次有修改就添加一条，带有 修改人，修改时间，修改描述）

 ***************************************************************************
 *   测试轨迹规划及取点程序运行时间
 * 对每种情况进行测试次数为：TEST_NUM
 * 取点测试过程，将相应的Pos、Vel、Acc数据写入相应.dat文件
 * 测试所选用模型为Puma560，六轴机器人
*                                                                         *
 ***************************************************************************/

#include "trajectoryTest.hpp"
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_composite.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/trajectory_stationary.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/utilities/error.h>

#define TEST_NUM 10000

using namespace KDL;
//using namespace std;

TrajectoryTest::TrajectoryTest()
{
    this->puma = Puma560();
}

TrajectoryTest::~TrajectoryTest()
{

}

void TrajectoryTest::pathConfigure(Trajectory* traj, TrajectoryTest::IdentifierType testType)
{
    try
    {
        //速度规划时间内取点，取一个点的平均时间
        double dt = traj->Duration() / 10.0;
        switch (testType)
        {
        case TYPE_POS:
            //速度规划时间内取点，取一个点的平均时间
            //double dt = traj->Duration() / 10.0;
            //取一个点
            for (double t = dt; t <= dt; t += dt)
            {
                Frame currentPos;
                currentPos = traj->Pos(t);
            }
            break;

        case TYPE_VEL:
            //速度规划时间内取点，取一个点的平均时间
            //double dt = traj->Duration() / 10.0;
            //取一个点
            for (double t = dt; t <= dt; t += dt)
            {
                Twist currentVel;
                currentVel = traj->Vel(t);
            }
            break;

        case TYPE_ACC:
            //速度规划时间内取点，取一个点的平均时间
            //double dt = traj->Duration() / 10.0;
            //取一个点
            for (double t = dt; t <= dt; t += dt)
            {
                Twist currentAcc;
                currentAcc = traj->Acc(t);
            }
            break;

        case TYPE_PVA:
            //速度规划时间内取点，取一个点的平均时间
            //double dt = traj->Duration() / 10.0;
            //取一个点
            for (double t = dt; t <= dt; t += dt)
            {
                Frame currentPos;
                Twist currentVel;
                Twist currentAcc;
                currentPos = traj->Pos(t);
                currentVel = traj->Vel(t);
                currentAcc = traj->Acc(t);
            }
            break;

        default:
            break;
        }

//        //速度规划时间内取点，取一个点的平均时间
//        double dt = traj->Duration() / 10.0;
//        //存放运行轨迹的Pos数据
//        std::ofstream ofp("./testData/trajectoryPos.dat");
//        //存放运行轨迹的Vel数据
//        std::ofstream ofv("./testData/trajectoryVel.dat");
//        //存放运行轨迹的Acc数据
//        std::ofstream ofa("./testData/trajectoryAcc.dat");
//        switch (testType)
//        {
//        case TYPE_POS:
//            //取一个点
//            for (double t = dt; t <= dt; t += dt)
//            {
//                Frame currentPos;
//                currentPos = traj->Pos(t);
//                //Pos数据
//                for (int i = 0; i < 4; i++)
//                {
//                    for(int j = 0; j < 4; j++)
//                    {
//                        ofp << currentPos(i, j) << "\t";
//                    }
//                }
//                ofp << "\n";
//            }
//            ofp.close();
//            break;

//        case TYPE_VEL:
//            //取一个点
//            for (double t = dt; t <= dt; t += dt)
//            {
//                Twist currentVel;
//                currentVel = traj->Vel(t);
//                //Vel数据
//                for (int i = 0; i < 6; i++)
//                {
//                    ofv << currentVel(i) << "\t";
//                }
//                ofv << "\n";
//            }
//            ofv.close();
//            break;

//        case TYPE_ACC:
//            //取一个点
//            for (double t = dt; t <= dt; t += dt)
//            {
//                Twist currentAcc;
//                currentAcc = traj->Acc(t);
//                //Acc数据
//                for (int i = 0; i  < 6; i++)
//                {
//                    ofa << currentAcc(i) << "\t";
//                }
//                ofa << "\n";
//            }
//            ofa.close();

//        case TYPE_PVA:
//            //取一个点
//            for (double t = dt; t <= dt; t += dt)
//            {
//                Frame currentPos;
//                Twist currentVel;
//                Twist currentAcc;
//                currentPos = traj->Pos(t);
//                currentVel = traj->Vel(t);
//                currentAcc = traj->Acc(t);

//                //Pos数据
//                for (int i = 0; i < 4; i++)
//                {
//                    for(int j = 0; j < 4; j++)
//                    {
//                        ofp << currentPos(i, j) << "\t";
//                    }
//                }
//                ofp << "\n";

//                //Vel数据
//                for (int i = 0; i < 6; i++)
//                {
//                    ofv << currentVel(i) << "\t";
//                }
//                ofv << "\n";

//                //Acc数据
//                for (int i = 0; i  < 6; i++)
//                {
//                    ofa << currentAcc(i) << "\t";
//                }
//                ofa << "\n";
//            }
//            ofp.close();
//            ofv.close();
//            ofa.close();
//            break;

//        default:
//            break;
//        }
    }

    catch(Error& error)
    {
        std::cout << "I encountered this error : " << error.Description() << std::endl;
        std::cout << "with the following type" << error.GetType() << std::endl;
    }
}

void TrajectoryTest::writeTrajectoryTestTime(std::string str, MeasureTime *timeTest)
{
    //outfile.open("./trajectoryTestTime.txt", std::ios::app); //std::ios::app表示在文件末尾追加
    outfile.open("./TestTime.txt", std::ios::app);
    //检查文件是否正常打开
    if(!outfile)
    {
       //std::cout <<"trajectoryTestTime.txt can't open" << std::endl;
        std::cout <<"TestTime.txt can't open" << std::endl;
       //打开失败，结束程序
       abort();
    }
    else
    {
        outfile << str ;
        outfile << "minTime=" << timeTest->minTime / 1000 << "us;" << "\t";
        outfile << "maxTime=" << timeTest->maxTime / 1000 << "us;" << "\t";
        outfile << "averageTime=" << timeTest->averageTime / 1000 << "us;" << "\t";
        outfile << "allTime=" << timeTest->addTime / 1000 << "us" << "\n" << std::endl;
        outfile.close();
    }
    timeTest->reset();
}

void TrajectoryTest::getPointTest(MeasureTime *timeTest, Trajectory *traj, TrajectoryTest::IdentifierType testType)
{
    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest->recordInTime();
        this->pathConfigure(traj, testType);
        timeTest->recordOutTime();
    }
}

void TrajectoryTest::pathPointTest()
{

}

void TrajectoryTest::pathLineTest()
{
    Frame fs, fe;
    ChainFkSolverPos_recursive fkPosSolver = ChainFkSolverPos_recursive(this->puma);
    int jn = this->puma.getNrOfJoints();
    JntArray jointStart = JntArray(jn);
    JntArray jointEnd = JntArray(jn);
    MeasureTime* timeTest1 = new MeasureTime("LINE轨迹时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest2 = new MeasureTime("LINE轨迹取点(Pos)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest3 = new MeasureTime("LINE轨迹取点(Vel)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest4 = new MeasureTime("LINE轨迹取点(Acc)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest5 = new MeasureTime("LINE轨迹取点(PVA)时间", 1, 1, TEST_NUM);
    Path_Line* path = NULL;
    VelocityProfile* velProf = NULL;
    Trajectory* traj = NULL;
    //随机指定各关节位置
    double tmp;
    for (int i = 0; i < jn; i++)
    {
        random(jointStart(i));
        random(tmp);
        jointEnd(i) = jointStart(i) + 0.1*tmp;
    }

    assert(fkPosSolver.JntToCart(jointStart, fs) == 0);
    std::cout << "start Frame:" << std::endl;
    std::cout << fs << std::endl;

    assert(fkPosSolver.JntToCart(jointEnd, fe) == 0);
    std::cout << "end Frame:" << std::endl;
    std::cout << fe << std::endl;

    std::cout << "LINE(trap velocity):" << std::endl;
    //测试LINE轨迹规划程序运行时间
    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Line(fs, fe, new RotationalInterpolation_SingleAxis(), 0.1);//eqradius=0.1
        velProf = new VelocityProfile_Trap(0.2, 0.4);
        velProf->SetProfile(0, path->PathLength());
        if (i == 0)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        //建立轨迹，输入路径类型path和速度规划类型velProf
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "LINE(trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Line(fs, fe, new RotationalInterpolation_SingleAxis(), 0.1);//eqradius=0.1
    velProf = new VelocityProfile_Trap(0.2, 0.4);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试LINE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "LINE(trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试LINE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "LINE(trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试LINE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "LINE(trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试LINE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "LINE(trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    std::cout << "LINE(half trap velocity):" << std::endl;

    for(int i=0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Line(fs, fe, new RotationalInterpolation_SingleAxis(), 0.1);//eqradius=0.1
        velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
        velProf->SetProfile(0, path->PathLength());
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "LINE(half trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Line(fs,fe,new RotationalInterpolation_SingleAxis(),0.1);//eqradius=0.1
    velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试LINE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "LINE(half trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试LINE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "LINE(half trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试LINE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "LINE(half trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试LINE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "LINE(half trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "LINE(spline velocity):" << std::endl;
    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Line(fs, fe, new RotationalInterpolation_SingleAxis(), 0.1);//eqradius=0.1
        velProfSpline = new VelocityProfile_Spline();
        velProfSpline->SetProfileDuration(0.0, 0.0,path->PathLength(), 0.0, 0.5);
        traj = new Trajectory_Segment(path, velProfSpline);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "LINE(spline velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Line(fs, fe, new RotationalInterpolation_SingleAxis(), 0.1);//eqradius=0.1
    velProfSpline = new VelocityProfile_Spline();
    velProfSpline->SetProfileDuration(0.0, 0.0,path->PathLength(), 0.0, 0.5);
    traj = new Trajectory_Segment(path, velProfSpline);

    //测试LINE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "LINE(spline velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试LINE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "LINE(spline velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试LINE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "LINE(spline velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试LINE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "LINE(spline velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    delete timeTest1;
    timeTest1 = NULL;
    delete timeTest2;
    timeTest2 = NULL;
    delete timeTest3;
    timeTest3 = NULL;
    delete timeTest4;
    timeTest4 = NULL;
    delete timeTest5;
    timeTest5 = NULL;
}

void TrajectoryTest::pathCircleTest()
{
    Frame fs, fe;
    Vector center;
    MeasureTime* timeTest1 = new MeasureTime("CIRCLE轨迹时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest2 = new MeasureTime("CIRCLE轨迹取点(Pos)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest3 = new MeasureTime("CIRCLE轨迹取点(Vel)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest4 = new MeasureTime("CIRCLE轨迹取点(Acc)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest5 = new MeasureTime("CIRCLE轨迹取点(PVA)时间", 1, 1, TEST_NUM);
    Path_Circle* path = NULL;
    VelocityProfile* velProf = NULL;
    Trajectory* traj = NULL;
    center = Vector(0, 0, 0);
    fs = Frame(Rotation::RPY(0.1, 0.2, 0.3), Vector(1, 0, 0));
    fe = Frame(Rotation::RPY(0.2, 0.3, 0.1), Vector(0, 1, 0));
    std::cout << "start Frame:" << std::endl;
    std::cout << fs << std::endl;
    std::cout << "end Frame:" << std::endl;
    std::cout << fe << std::endl;

    std::cout << "CIRCLE(trap velocity):" <<std::endl;

    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
        velProf = new VelocityProfile_Trap(0.2, 0.2);
        velProf->SetProfile(0, path->PathLength());
        if(i == 0)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "CIRCLE(trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
    velProf = new VelocityProfile_Trap(0.2, 0.2);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试CIRCLE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "CIRCLE(trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试CIRCLE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "CIRCLE(trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试CIRCLE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "CIRCLE(trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试CIRCLE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "CIRCLE(trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    std::cout << "CIRCLE(half trap velocity):" <<   std::endl;

    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
        velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
        velProf->SetProfile(0, path->PathLength());
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "CIRCLE(half trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
    velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试CIRCLE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "CIRCLE(half trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试CIRCLE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "CIRCLE(half trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试CIRCLE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "CIRCLE(half trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试CIRCLE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "CIRCLE(half trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "CIRCLE(spline velocity):" << std::endl;

    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
        velProfSpline = new VelocityProfile_Spline();
        velProfSpline->SetProfileDuration(0.0, 0.0, path->PathLength(), 0.0, 1.4);
        traj = new Trajectory_Segment(path, velProfSpline);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "CIRCLE(spline velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(), 0.1);
    velProfSpline = new VelocityProfile_Spline();
    velProfSpline->SetProfileDuration(0.0, 0.0, path->PathLength(), 0.0, 1.4);
    traj = new Trajectory_Segment(path, velProfSpline);

    //测试CIRCLE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "CIRCLE(spline velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试CIRCLE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "CIRCLE(spline velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试CIRCLE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "CIRCLE(spline velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试CIRCLE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "CIRCLE(spline velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    delete timeTest1;
    timeTest1 = NULL;
    delete timeTest2;
    timeTest2 = NULL;
    delete timeTest3;
    timeTest3 = NULL;
    delete timeTest4;
    timeTest4 = NULL;
    delete timeTest5;
    timeTest5 = NULL;
}

void TrajectoryTest::pathRoundedCompositeTest()
{
    Frame fs, fe;
    ChainFkSolverPos_recursive fkPosSolver = ChainFkSolverPos_recursive(this->puma);
    int jn = this->puma.getNrOfJoints();
    JntArray jointStart = JntArray(jn);
    JntArray jointEnd = JntArray(jn);
    MeasureTime* timeTest1 = new MeasureTime("ROUND_COMPOSITE轨迹时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest2 = new MeasureTime("ROUND_COMPOSITE轨迹取点(Pos)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest3 = new MeasureTime("ROUND_COMPOSITE轨迹取点(Vel)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest4 = new MeasureTime("ROUND_COMPOSITE轨迹取点(Acc)时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest5 = new MeasureTime("ROUND_COMPOSITE轨迹取点(PVA)时间", 1, 1, TEST_NUM);
    Path_RoundedComposite* path = NULL;
    VelocityProfile* velProf = NULL;
    Trajectory* traj = NULL;
    double tmp;
    for (int i = 0; i < jn; i++)
    {
        random(jointStart(i));
        random(tmp);
        jointEnd(i) = jointStart(i) + 0.1*tmp;
    }

    assert(fkPosSolver.JntToCart(jointStart, fs) == 0);
    std::cout << "start Frame:" << std::endl;
    std::cout << fs <<std::endl;

    assert(fkPosSolver.JntToCart(jointEnd, fe)==0);
    std::cout << "end Frame:" << std::endl;
    std::cout << fe <<std::endl;

    std::cout << "ROUND_COMPOSITE(trap velocity):" << std::endl;

    //测试ROUND_COMPOSITE轨迹规划程序运行时间
    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProf = new VelocityProfile_Trap(0.2, 0.2);
        velProf->SetProfile(0, path->PathLength());
        if(i == 0)
        {
            std::cout << "pathLength:" << path->PathLength() << std::endl;
            std::cout << "duration: " << velProf->Duration() << std::endl;
        }
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "ROUND_COMPOSITE(trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
    path->Add(fs);
    path->Add(fe);
    path->Finish();
    velProf = new VelocityProfile_Trap(0.2, 0.2);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试ROUND_COMPOSITE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "ROUND_COMPOSITE(trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试ROUND_COMPOSITE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "ROUND_COMPOSITE(trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试ROUND_COMPOSITE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "ROUND_COMPOSITE(trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试ROUND_COMPOSITE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "ROUND_COMPOSITE(trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    std::cout << "ROUND_COMPOSITE(half trap velocity):" << std::endl;

    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
        velProf->SetProfile(0, path->PathLength());
        traj = new Trajectory_Segment(path, velProf);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "ROUND_COMPOSITE(half trap velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
    path->Add(fs);
    path->Add(fe);
    path->Finish();
    velProf = new VelocityProfile_TrapHalf(0.2, 0.2);
    velProf->SetProfile(0, path->PathLength());
    traj = new Trajectory_Segment(path, velProf);

    //测试ROUND_COMPOSITE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "ROUND_COMPOSITE(half trap velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试ROUND_COMPOSITE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "ROUND_COMPOSITE(half trap velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试ROUND_COMPOSITE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "ROUND_COMPOSITE(half trap velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试ROUND_COMPOSITE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "ROUND_COMPOSITE(half trap velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "ROUND_COMPOSITE(spline velocity):" << std::endl;

    for (int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProfSpline = new VelocityProfile_Spline( );
        velProfSpline->SetProfileDuration(0.0, 0.0, path->PathLength(), 0.0, 0.5);
        traj = new Trajectory_Segment(path, velProfSpline);
        timeTest1->recordOutTime();
        delete traj;
        traj = NULL;
    }
    str = "ROUND_COMPOSITE(spline velocity)轨迹规划测试时间：";
    writeTrajectoryTestTime(str, timeTest1);

    path = new Path_RoundedComposite(0.2, 0.1, new RotationalInterpolation_SingleAxis());
    path->Add(fs);
    path->Add(fe);
    path->Finish();
    velProfSpline = new VelocityProfile_Spline( );
    velProfSpline->SetProfileDuration(0.0, 0.0, path->PathLength(), 0.0, 0.5);
    traj = new Trajectory_Segment(path, velProfSpline);

    //测试ROUND_COMPOSITE取点(Pos)程序运行时间
    getPointTest(timeTest2, traj, TYPE_POS);
    str = "ROUND_COMPOSITE(spline velocity)取点(Pos)测试时间：";
    writeTrajectoryTestTime(str, timeTest2);

    //测试ROUND_COMPOSITE取点(Vel)程序运行时间
    getPointTest(timeTest3, traj, TYPE_VEL);
    str = "ROUND_COMPOSITE(spline velocity)取点(Vel)测试时间：";
    writeTrajectoryTestTime(str, timeTest3);

    //测试ROUND_COMPOSITE取点(Acc)程序运行时间
    getPointTest(timeTest4, traj, TYPE_ACC);
    str = "ROUND_COMPOSITE(spline velocity)取点(Acc)测试时间：";
    writeTrajectoryTestTime(str, timeTest4);

    //测试ROUND_COMPOSITE取点(PVA)程序运行时间
    getPointTest(timeTest5, traj, TYPE_PVA);
    str = "ROUND_COMPOSITE(spline velocity)取点(PVA)测试时间：";
    writeTrajectoryTestTime(str, timeTest5);

    delete traj;
    traj = NULL;

    delete timeTest1;
    timeTest1 = NULL;
    delete timeTest2;
    timeTest2 = NULL;
    delete timeTest3;
    timeTest3 = NULL;
    delete timeTest4;
    timeTest4 = NULL;
    delete timeTest5;
    timeTest5 = NULL;
}
